#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "MPU6050.h"
#include "Serial.h"
#include "Timer.h"
#include <math.h>
#include "Attitude_algorithm.h"
uint8_t RxData;			//定义用于接收串口数据的变量
uint8_t ID;								//定义用于存放ID号的变量
MPU mpu;
//float t = 0;

/*计算偏移量*/
float i;                                    //计算偏移量时的循环次数
float ax_offset = 0, ay_offset = 0;         //x,y轴的加速度偏移量
float gx_offset = 0, gy_offset = 0;         //x,y轴的角速度偏移量
int N = 200;
/*参数*/
float rad2deg = 57.29578;                   //弧度到角度的换算系数
float roll_v = 0, pitch_v = 0;              //换算到x,y轴上的角速度

/*定义微分时间*/
float dt = .001f;        //定义微分时间

/*三个状态，先验状态，观测状态，最优估计状态*/
float gyro_roll = 0, gyro_pitch = 0;        //陀螺仪积分计算出的角度，先验状态
float acc_roll = 0, acc_pitch = 0;          //加速度计观测出的角度，观测状态
float roll = 0, pitch = 0;              //卡尔曼滤波后估计出最优角度，最优估计状态

/*误差协方差矩阵P*/
float e_P[2][2] ={{1,0},{0,1}};             //误差协方差矩阵，这里的e_P既是先验估计的P，也是最后更新的P

/*卡尔曼增益K*/
float k_k[2][2] ={{0,0},{0,0}};             //这里的卡尔曼增益矩阵K是一个2X2的方阵
double sqrt3(double x) { 
	//float后加f转换成double类型
    if(x == 0) return 0; 
    float result = x; 
    float xhalf = 0.5f*result; 
    int i = *(int*)&result; 
    
    // what the fuck? 
    i = 0x5f375a86 - (i>>1); 
    result = *(float*)&i; 
    result = result*(1.5f-xhalf*result*result);
    result = result*(1.5f-xhalf*result*result); 
    return 1.0f/result; 
}
int main(void)
{
	Delay_ms(100);
	/*模块初始化*/
	MPU6050_Init();		//MPU6050初始化
	ID = MPU6050_GetID();				//获取MPU6050的ID号
	/*串口初始化*/
	Serial_Init();		//串口初始化
	//计算偏移量
  for (i = 1; i <= N; i++) {
    MPU6050_GetData(&mpu);//获取加速度、角速度、温度
    ax_offset += mpu.ax;//计算x轴加速度的偏移总量
    ay_offset += mpu.ay;//计算y轴加速度的偏移总量
    gx_offset += mpu.gx;
    gy_offset += mpu.gy;
  }
  ax_offset /= N; //计算x轴加速度的偏移量
  ay_offset /= N; //计算y轴加速度的偏移量
  gx_offset /= N; //计算x轴角速度的偏移量
  gy_offset /= N; //计算y轴角速度的偏移量
//	Timer_Init();
	
	
	while (1)
	{
		if (ID){
			MPU6050_GetData(&mpu);		//获取MPU6050的数据
			/*step1:计算先验状态*/
			/*计算x,y轴上的角速度*/
			roll_v = (mpu.gx-gx_offset) + tan(gyro_pitch)*sin(gyro_roll)*(mpu.gy-gy_offset) + tan(gyro_pitch)*cos(gyro_roll)*mpu.gz;//roll轴的角速度
			pitch_v = cos(gyro_roll)*(mpu.gy-gy_offset) - sin(gyro_roll)*mpu.gz;//pitch轴的角速度
			gyro_roll += dt*roll_v;//先验roll角度
			gyro_pitch += dt*pitch_v;//先验pitch角度

			/*step2:计算先验误差协方差矩阵P*/
			e_P[0][0] = e_P[0][0] + 0.3;//这里的Q矩阵是一个对角阵且对角元均为0.0025
			e_P[0][1] = e_P[0][1] + 0;
			e_P[1][0] = e_P[1][0] + 0;
			e_P[1][1] = e_P[1][1] + 0.3;

			/*step3:更新卡尔曼增益K*/
			k_k[0][0] = e_P[0][0]/(e_P[0][0]+0.0025);
			k_k[0][1] = 0;
			k_k[1][0] = 0;
			k_k[1][1] = e_P[1][1]/(e_P[1][1]+0.0025);

			/*step4:计算最优估计状态*/
			/*观测状态*/
			//roll角度
			acc_roll = atan((mpu.ay - ay_offset) / (mpu.az));
			//pitch角度
			acc_pitch = -1*atan((mpu.ax - ax_offset) / sqrt3((mpu.ay - ay_offset)*(mpu.ay - ay_offset) + (mpu.az)*(mpu.az)));
			/*最优估计状态*/
			gyro_roll = (1-k_k[0][0]) * gyro_roll + k_k[0][0]*acc_roll;
			gyro_pitch = (1-k_k[0][0]) * gyro_pitch + k_k[1][1]*acc_pitch;
			
			roll = gyro_roll*rad2deg;
			pitch = gyro_pitch*rad2deg;
			
			/*step5:更新协方差矩阵P*/
			e_P[0][0] = (1 - k_k[0][0])*e_P[0][0];
			e_P[0][1] = 0;
			e_P[1][0] = 0;
			e_P[1][1] = (1 - k_k[1][1])*e_P[1][1];
//			kalman_filter_solution(&mpu);
//			printf("%d,%d,%d,%d,%d,%d\n",mpu.ax,mpu.ay,mpu.az,mpu.gx,mpu.gy,mpu.gz);
			printf("%f,%f\n",roll,pitch);
		}
		
	}
	
}

//void TIM2_IRQHandler(void)
//{
//	if (TIM_GetITStatus(TIM2, TIM_IT_Update) == SET)		//判断是否是TIM2的更新事件触发的中断
//	{
//		MPU6050_GetData(&mpu);		//获取MPU6050的数据
//		/*计算x,y轴上的角速度*/
//		roll_v = (mpu.gx-gx_offset) + ((sin(k_pitch)*sin(k_roll))/cos(k_pitch))*(mpu.gy-gy_offset) + ((sin(k_pitch)*cos(k_roll))/cos(k_pitch))*mpu.gz;//roll轴的角速度
//		pitch_v = cos(k_roll)*(mpu.gy-gy_offset) - sin(k_roll)*mpu.gz;//pitch轴的角速度
//		gyro_roll += dt*roll_v;//先验roll角度
//		gyro_pitch += dt*pitch_v;//先验pitch角度
////		LED1_Turn();					
//		TIM_ClearITPendingBit(TIM2, TIM_IT_Update);			//清除TIM2更新事件的中断标志位
//															//中断标志位必须清除
//															//否则中断将连续不断地触发，导致主程序卡死
//	}
//}
